#!/usr/bin/env python

import roslib; roslib.load_manifest('framer')
from random import choice
from framer.msg import *
import rospy
import actionlib
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv
import smach
from smach_ros import ActionServerWrapper
from exif import Exif
from picasa.srv import ImageUpload
from framer.msg import Status
import time
from backgroundProcessor import *
from faces.msg import Faces

class PictureTaker(smach.State):
   def __init__(self):
      smach.State.__init__(self,
         outcomes=['success', 'fail', 'abort', 'request'],
         output_keys=['cue'],
         input_keys=['faces'],
      )
      self.current_pic = None
      self.bridge = CvBridge()
      self.process_pic = None
      self.background = BackgroundProcessor() # Defaults to threshold of 15 and standard hough
      rospy.Subscriber('/wide_stereo/right/image_rect_color', Image, self.pic_callback)
      rospy.Subscriber('/pro_out/image_rect_color', Image, self.pic_callback)
      self.status_pub = rospy.Publisher('/framer/status', Status)
      self.uploader = None
      self.lastPicture = 0
      self.direction = None
      self.photo_pub = rospy.Publisher('/framer/photograph', Faces)
      try:
          rospy.wait_for_service('upload_image',5)
          self.uploader = rospy.ServiceProxy('upload_image', ImageUpload)
      except rospy.ROSException, e:
          rospy.loginfo('No uploader service, photos will not be uploaded.')
      self.num = 0 
      # @TODO make this configurable
      # maybe take this is as a rosparam from the launch file. That way you can have it automatically find the correct path
      self.directory = '/home/mfollett/class_project/robot-photographer'
      self.directory = '/u/cse553/ros/robotic-photographer-read-only'
      self.exif = Exif('Harris the Robot', 'Washington University, 2012')

   def pic_callback(self, image):
      self.current_image = image
      self.current_pic = self.bridge.imgmsg_to_cv(image, 'bgr8')
      self.process_pic = self.bridge.imgmsg_to_cv(image, 'mono8')

   def reset_direction(self):
       self.direction = None

   def pick_direction(self):
      # if a direction has been previously declared
      # continue that direction so as not to frustrate
      # the humans
      if not self.direction:
          self.direction = choice(['left', 'right'])
      return self.direction

   def execute(self, userdata):
      self.status_pub.publish(Status(Status.TAKING_PHOTO))
      rospy.loginfo("Taking a picture.")
      if rospy.is_shutdown():
         return 'abort'
      # Wait until process_pic isn't None?
      if self.background.process(self.process_pic) == "busy":
         userdata.cue = 'move_' + self.pick_direction()
         return 'request'
      success = self.takePicture(userdata.faces)
      if success:
         return 'success'
      return 'fail'
      

   def uploadPicture(self, picture):
       if not self.uploader:
           return True
       try:
           self.uploader(picture, '', '', '')
       except rospy.ServiceException, e:
           rospy.loginfo('Failed to upload photo: %s' % e)
           return False
       return True

   def takePicture(self, faces):
      if self.current_pic == None:
         rospy.loginfo("Current image unavailable, cannot save. Skipping.")
         return False
      if self.lastPicture + 1 > time.clock():
         return True
      self.lastPicture = time.clock()
      file_location = "{0}/output/picture{1}.jpg".format(self.directory, self.num)
      cv.SaveImage(file_location, self.current_pic)
      self.num += 1

      self.reset_direction()
      #can't really think of any useful debugging information yet
      self.exif.tag(file_location, {'photo_number': self.num})

      faces.image = self.current_image
      self.photo_pub.publish(faces)
      return self.uploadPicture(file_location)
    
if __name__ == '__main__':
    rospy.init_node('picture_taker')
    machine = smach.StateMachine(outcomes=['success', 'fail', 'abort'])
    with machine:
        smach.StateMachine.add('Picture Taker', PictureTaker())
    ActionServerWrapper(
        'picture_taker',
        TakePictureAction,
        wrapped_container = machine,
        succeeded_outcomes = ['success'],
        aborted_outcomes = ['fail', 'aborted'],
        preempted_outcomes = [],
    ).run_server()
    rospy.loginfo('spinning')
    rospy.spin()
